#include "stdio.h"
#include "485_slave.h"
#include "string.h"
#include "math.h"
#include "stdlib.h"

// slave485_t screen;
slave485_err_t slave485_init(slave485_t *slave485, uint8_t addr, uint32_t baudrate)
{
    if (slave485 == NULL)
        return SLAVE485_ERR_INVALID_ARG;
    memset(slave485, 0, sizeof(slave485_t));
    slave485->addr = addr;
    slave485->baudrate = baudrate;
    return SLAVE485_OK;
}

slave485_err_t slave485_deinit(slave485_t *slave485)
{
    return SLAVE485_OK;
}

slave485_err_t slave485_analyze(slave485_t *slave485, uint8_t *data, uint16_t size)
{
    return SLAVE485_OK;
}

slave485_err_t slave485_add_reg(slave485_t *slave485, slave_reg_t *reg)
{
    if (slave485 == NULL || reg == NULL)
    {
        return SLAVE485_ERR_INVALID_ARG;
    }
    slave_reg_t *tmp = &slave485->reg;
    while (tmp->next != NULL)
    {
        for (int i = 0; i < tmp->next->size; i++)
            if (tmp->next->addr + i == reg->addr)
                return SLAVE485_ERR_INVALID_ARG; // register already exists.
        tmp = tmp->next;
    }
    tmp->next = malloc(sizeof(slave_reg_t));
    if (tmp->next == NULL)
        return SLAVE485_ERR_NO_MEM;
    memset(tmp->next, 0, sizeof(slave_reg_t));
    tmp->next->addr = reg->addr;
    tmp->next->data = malloc(reg->size * sizeof(uint16_t));
    if (tmp->next->data == NULL)
    {
        free(tmp->next);
        tmp->next = NULL;
        return SLAVE485_ERR_NO_MEM;
    }
    memset(tmp->next->data, 0x00, reg->size * sizeof(uint16_t));
    tmp->next->size = reg->size;
    tmp->next->update = false;
    tmp->next->reg_cb = reg->reg_cb;
    return SLAVE485_OK;
}

slave485_err_t slave485_remove_reg(slave485_t *slave485, uint16_t addr)
{
    if (slave485 == NULL)
    {
        return SLAVE485_ERR_INVALID_ARG;
    }
    slave_reg_t *temp = &slave485->reg;
    while (temp->next != NULL)
    {
        if (temp->next->addr == addr)
        {
            slave_reg_t *temp2 = temp->next->next;
            free(temp->next->data);
            free(temp->next);
            temp->next = temp2;
            return SLAVE485_OK;
        }
        temp = temp->next;
    }
    return SLAVE485_OK;
}

slave485_err_t slave485_read_reg(slave485_t *slave485, uint16_t addr, uint16_t *data, uint16_t size)
{
    if (slave485 == NULL)
    {
        return SLAVE485_ERR_INVALID_ARG;
    }
    slave_reg_t *temp = &slave485->reg;
    // bool found = false;
    while (temp->next != NULL)
    {
        for (int i = 0; i < temp->next->size; i++)
        {
            if (temp->next->addr + i == addr)
            {
                if (size > temp->next->size - i)
                {
                    printf("slave485 Invalid argument, size: %d", size);
                    return SLAVE485_ERR_INVALID_ARG;
                }
                memcpy(data, &temp->next->data[i], size * sizeof(uint16_t));
                // found = true;
                return SLAVE485_OK;
            }
        }
        temp = temp->next;
    }
    return SLAVE485_ERR_REG_NOT_FOUND;
    // return found ? SLAVE485_OK : SLAVE485_ERR_REG_NOT_FOUND;
}

slave485_err_t slave485_write_reg(slave485_t *slave485, uint16_t addr, uint16_t data)
{
    if (slave485 == NULL)
    {
        return SLAVE485_ERR_INVALID_ARG;
    }
    slave_reg_t *temp = &slave485->reg;
    bool found = false;
    while (temp->next != NULL)
    {
        for (int i = 0; i < temp->next->size; i++)
        {
            if (temp->next->addr + i == addr)
            {
                memcpy(&temp->next->data[i], &data, sizeof(uint16_t));
                // temp->next->data[i] = data;
                found = true;
                if (temp->next->reg_cb != NULL)
                {
                    temp->next->reg_cb(temp->next->data);
                }
                return SLAVE485_OK;
            }
        }
        temp = temp->next;
    }
    // return SLAVE485_ERR_REG_NOT_FOUND;
    return found ? SLAVE485_OK : SLAVE485_ERR_REG_NOT_FOUND;
}

slave485_err_t slave485_add_coil(slave485_t *slave485, slave_coil_t *coil)
{
    if (slave485 == NULL || coil == NULL)
    {
        return SLAVE485_ERR_INVALID_ARG;
    }
    slave_coil_t *tmp = &slave485->coil;
    while (tmp->next != NULL)
    {
        for (int i = 0; i < coil->size; i++)
            if (tmp->next->addr + i == coil->addr)
                return SLAVE485_ERR_INVALID_ARG; // coil already exists.
        tmp = tmp->next;
    }
    tmp->next = malloc(sizeof(slave_coil_t));
    if (tmp->next == NULL)
        return SLAVE485_ERR_NO_MEM;
    memset(tmp->next, 0, sizeof(slave_coil_t));
    tmp->next->addr = coil->addr;
    tmp->next->data = malloc(coil->size * sizeof(uint16_t));
    if (tmp->next->data == NULL)
    {
        free(tmp->next);
        tmp->next = NULL;
        return SLAVE485_ERR_NO_MEM;
    }
    memset(tmp->next->data, 0, coil->size * sizeof(uint16_t));
    tmp->next->size = coil->size;
    return SLAVE485_OK;
}

slave485_err_t slave485_read_coil(slave485_t *slave485, uint16_t addr, uint8_t *data, uint16_t size)
{
    if (slave485 == NULL)
    {
        return SLAVE485_ERR_INVALID_ARG;
    }
    slave_coil_t *temp = &slave485->coil;
    int data_len;
    // bool found = false;
    while (temp->next != NULL)
    {
        for (int i = 0; i < temp->next->size; i++)
            if (temp->next->addr + i == addr)
            {
                if (size > temp->next->size - i)
                    return SLAVE485_ERR_INVALID_ARG;
                if (size % 8 == 0)
                    data_len = size / 8;
                else
                    data_len = size / 8 + 1;
                // data = malloc(data_len);
                for (int j = 0; j < data_len; j++)
                {
                    data[j] = 0x00;
                    int byte_effective_len = fmin(8, size - 8 * j);
                    for (int k = 0; k < byte_effective_len; k++)
                        data[j] |= temp->next->data[j * 8 + k] == 0xFF00 ? (0x01 << k) : 0x00;
                }
                // found = true;
                return SLAVE485_OK;
            }
        temp = temp->next;
    }
    return SLAVE485_ERR_COIL_NOT_FOUND;
    // return found ? SLAVE485_OK : SLAVE485_ERR_REG_NOT_FOUND;
}

slave485_err_t slave485_write_coil(slave485_t *slave485, uint16_t addr, uint16_t data)
{
    if (slave485 == NULL)
    {
        return SLAVE485_ERR_INVALID_ARG;
    }
    slave_coil_t *temp = &slave485->coil;
    // bool found = false;
    while (temp->next != NULL)
    {
        for (int i = 0; i < temp->next->size; i++)
        {
            if (temp->next->addr + i == addr)
            {
                // memcpy(&temp->next->data[i], data, sizeof(uint16_t));
                temp->next->data[i] = data;
                return SLAVE485_OK;
            }
        }
        temp = temp->next;
    }
    return SLAVE485_ERR_COIL_NOT_FOUND;
    // return found ? SLAVE485_OK : SLAVE485_ERR_REG_NOT_FOUND;
}
